/* Autostop
 *
 * Copyright (c) 2006 - 2007 Maciej Gajewski, Grzegorz Latarowski
 * http://code.google.com/p/autostop-automaty-drogowe
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version. NO WARRANTY OF ANY KIND is expressed or implied.
 */

// camera
#include "camera.h"

// local
#include "imageanalyzer.h"
#include "gexception.h"
#include "globaldata.h"
#include "constants.h"


// ======================== run =====================
void ImageAnalyzer::run()
{
	// intialzie
	init();
	
	// analyze
	
	// allocate image data buffer
	GlobalData& global = GlobalData::instance;
	
	pimage_t	p_image		= create_image( camera_image_width(), camera_image_height() );
	int			bufferSize	= camera_image_width() * camera_image_height();
	int			max_speed	= 50; // TODO read fromsettings
	
	global.imageMutex.lock();
		global.image = p_image;
	global.imageMutex.unlock();
	
	if ( ! p_image )
	{
		throw GSysError("Error allocating frame buffer");
	}

	
	//  Main loop - one iteration per frame
	while ( 1 )
	{
		int cameraResult	= CAMERA_OK;	// result returned by camera routines
		
		//  Obtain image from driver
		global.imageMutex.lock();
			cameraResult = camera_get_image( p_image->pixels, bufferSize, &global.imageTimestamp );
		
		if ( CAMERA_OK != cameraResult )
		{
			global.imageMutex.unlock();
			QString msg = QString("Error obtaining image from camera: %1").arg( camera_error_message() );
			throw GConfigurationError( msg );
		}

		// Add image to process
		add_frame( _pProcess, p_image, global.imageTimestamp );
		
		// if reqested, fill current frame with bg/fr differnece
		if ( _showDiff )
		{
			prepareDiff( p_image );
		}
		global.imageNew = true;
		global.imageMutex.unlock();

		// Search process output for interesting moving object

		int object	= 0;			// object loop counter
		for( object = 0; object < _pProcess->object_number; object ++ )
		{
			object_t* p_object = _pProcess->objects + object;
			// if object has interesting speed, report it and remove from process
			if ( p_object->speed > 0 )
			{
				qDebug("speed measured: %d", (int)p_object->speed );
			}
			
			// TODO remove only old objects
			if ( p_object->speed > max_speed )
			{
				reportObject( p_object );
				remove_object( _pProcess, object );
				break; // one object per frame is enough for now
				// if not, we will have to deal with issues with changing indexes
			}
			
		} // object loop

		// check for incoming params
		if ( global.paramsNew )
		{
			global.paramsMutex.lock();
				readParams( global.params );
				global.paramsNew = false;
			global.paramsMutex.unlock();
		}

	} // main loop

	// basically... that's all, folks!

}

// ======================== init =====================
void ImageAnalyzer::init()
{
	// memebers init
	_showDiff = false;

	// create anaalyzer process
	qDebug("Initializing process");
	_pProcess = create_process();
	
	if ( ! _pProcess )
	{
		throw GConfigurationError("Error creating process");
	}
	
	// load markers
	FILE* fileMarkers = fopen( MARKERS_FILE, "r" );
	qDebug("Loading markers");
	if ( fileMarkers )
	{
		qDebug("Markers found in %s", MARKERS_FILE );
		
		_pProcess->pdistance_markers = load_image( fileMarkers );
		fclose( fileMarkers );
	}
	else
	{
		QString msg = QString("No markers file found (%1)").arg( MARKERS_FILE );
		throw GSysError( msg );
	}
	
	// load roi
	FILE* fileROI = fopen( ROI_FILE, "r" );
	qDebug("Loading ROI");
	if ( fileROI )
	{
		qDebug("ROI found in %s", ROI_FILE );
		
		_pProcess->pROI = load_image( fileROI );
		fclose( fileROI );
	}
	else
	{
		QString msg = QString("No ROI file found (%1)").arg( ROI_FILE );
		throw GSysError( msg );
	}
	
	// init camera device
	qDebug("Initializaing camera");
	if ( CAMERA_ERROR == camera_init() )
	{
		QString msg = QString( "Error initilizing camera: %1" ).arg( camera_error_message() );
		throw GConfigurationError( msg );
	}
	
	// lopad params
	// TODO actually load
	_pProcess->reference_distance = 100000;	// 100m
	
	// TODO other initilaization here (SQL!)
}

// ======================== reportObject =====================
void ImageAnalyzer::reportObject( pobject_t p_object )
{
	// TODO
	qDebug("reporting object...");
}

// ======================== readParams =====================
void ImageAnalyzer::readParams( const QVariantMap& params )
{
	if ( _pProcess )
	{
		const char* bgfactor = "bgfactor";
		const char* fgtreshold = "fgtreshold";
		const char* maxgroupdistance = "maxgroupdistance";
		const char* diff = "diff";
		
		// bgfactor
		if ( params.contains( bgfactor ) )
		{
			_pProcess->params.background_factor = params.value( bgfactor ).toDouble();
			// this line od got from analyzer insisde. This is manipulating on private 
			// data, this is ugly!
			_pProcess->_int_background_factor
				= (int)( (1<<16) * _pProcess->params.background_factor );
			qDebug("bfactor set to %lf (%d)"
				, _pProcess->params.background_factor
				, _pProcess->_int_background_factor
				 );
		}
		
		// treshold
		if ( params.contains( fgtreshold ) )
		{
			_pProcess->params.foreground_treshold = params.value( fgtreshold ).toInt();
			qDebug("fgtreshold to %d", (int)_pProcess->params.foreground_treshold );
		}
		
		// max distance
		if ( params.contains( maxgroupdistance ) )
		{
			_pProcess->params.max_group_distance = params.value( maxgroupdistance ).toInt();
			qDebug("maxgroupdistance to %d", (int)_pProcess->params.max_group_distance );
		}
		
		// show difference
		if ( params.contains( diff ) )
		{
			_showDiff = params.value( diff ).toBool();
			qDebug("diff to %d", (int)_showDiff );
		}
	}
}

// ======================== readParams =====================
/// Fill image with its own difference from process backgorund
void ImageAnalyzer::prepareDiff( pimage_t pImage )
{
	if ( _pProcess )
	{
		pixel_t* pBackgroundPixel = _pProcess->pbackground->pixels;
		foreach_pixel( pPixel, pImage )
		{
			*pPixel = abs( *pPixel - *pBackgroundPixel );
			pBackgroundPixel++;
		}
	}
}


// eof

